/* Includes -------------------------------------------------------------*/
#include "tamagawa.h"

#define STORE_SIZE 10
extern int32_T _turns_count;
extern int32_T _last_position;

extern int32_T _last_position_storage[STORE_SIZE];
extern int16_T _position_index;

/* Private Function Declaration -----------------------------------------*/
static void scib_Init(void);
static void scib_Transmit(char* pmsg, int msglen);
static uint16_T scib_Receive(uint16_T *rcvBuff, uint16_T buffLen);


/* Public Functions -----------------------------------------------------*/
/*
 * Function name	: Tamagawa_Init
 * Description		: 
 * Input			:
 * Output			:
 */
void Tamagawa_Init(void)
{
    uint16_T i=0;
#ifndef MATLAB_MEX_FILE
    scib_Init();
    
    EALLOW;
    GpioCtrlRegs.GPBPUD.bit.GPIO40 = 0;     /*Enable pull-up for GPIO40*/
    GpioCtrlRegs.GPBMUX1.bit.GPIO40 = 2;    /*Configure GPIO40 as SCITXDB*/
    GpioCtrlRegs.GPBPUD.bit.GPIO41 = 0;     /*Enable pull-up for GPIO41*/
    GpioCtrlRegs.GPBMUX1.bit.GPIO41 = 2;    /*Configure GPIO41 as SCIRXDB*/
    GpioCtrlRegs.GPBMUX1.bit.GPIO42 = 0;
    GpioCtrlRegs.GPBDIR.bit.GPIO42 = 1;
	EDIS;
    
    
    Tamagawa_GetSTPosition(&_last_position);
    for(i=0;i<STORE_SIZE;i++){
        _last_position_storage[i] = _last_position;
    }
        
#endif
}

/*
 * Function name	: Tamagawa_GetSpeed_and_ElecAngle
 * Description		: Get speed and ElecAngle
 * Input			:
 * Output			:
 */

int32_T _turns_count = 0;
int32_T _last_position = 0;
int32_T Tamagawa_Get_Absolute_Position(int32_T *Position)
{

    int32_T abs_position = 0;
    int32_T error_position = 0;
    
    error_position = (*Position) - _last_position;
    
    if(error_position > 65536L || error_position < -65536L ){
        
        _turns_count += error_position<0?1:-1;
        
    }
    _last_position = *Position;
    
    abs_position = _turns_count*131072 + (*Position);// - _turns_count>0?0:131072;
    
    return abs_position;
}

// int32_T Tamagawa_Get_Absolute_Position(int32_T *Position)
// {
// 
//     int32_T abs_position = 0;
//     int32_T error_position = 0;
//     int32_T abs_error_position = 0;
//     
//     error_position = (*Position) - _last_position;
//     
//     if(error_position < 0L){
//         abs_error_position = -error_position;
//     }else{
//         abs_error_position = error_position;
//     }
//     
//     if(abs_error_position > 65536L){
//         
//         if(error_position < 0L){
//             _turns_count++;
//         }else{
//             _turns_count--;
//         }
//         
//     }
//     _last_position = *Position;
//     
//     abs_position = _turns_count*131072 + (*Position);// - _turns_count>0?0:131072;
//     
//     return abs_position;
// }

/*
 * Function name	: Tamagawa_GetSTPosition
 * Description		: 
 * Input			:
 * Output			:
 */
uint16_T Tamagawa_GetSTPosition(int32_T *Position)
{
    uint16_T errFlg = NOERROR;	
    
#ifndef MATLAB_MEX_FILE
	uint16_T i;
    uint16_T TxBuff, RxBuff[6];
    
    TxBuff = 0x02;
    for (i=0; i<6; i++)
        RxBuff[i] = 0;
    
    GpioDataRegs.GPBSET.bit.GPIO42 = 1;
    
    ScibRegs.SCIFFRX.bit.RXFIFORESET = 0;    /* Reset the FIFO pointer to zero.   */
    ScibRegs.SCIFFRX.bit.RXFIFORESET = 1;    /* Re-enable receive FIFO operation. */
    
    scib_Transmit((char*)&TxBuff, 1);
    
    GpioDataRegs.GPBCLEAR.bit.GPIO42 = 1;
    
    errFlg = scib_Receive(RxBuff, 6);
    
    if (errFlg == NOERROR)
    {
        *Position = (((int32_T)RxBuff[4] << 16) | ((int32_T)RxBuff[3] << 8) | (int32_T)RxBuff[2]) & 0x0001FFFF;
    }
    else
    {
        *Position = 0x00000000;
    }
    
    *Position = Tamagawa_Get_Absolute_Position(Position);
#endif
    
    return errFlg;
}

/*
 * Function name	: Tamagawa_GetSpeed_and_ElecAngle
 * Description		: Get speed and ElecAngle
 * Input			:
 * Output			:
 */


#define Motor_PolePairCount 32768L
#define PI 3.141592654
#define Angle_Offset 0L

int32_T _last_position_storage[STORE_SIZE] = {0};
int16_T _position_index = 0;
int32_T Tamagawa_Get_Last10_Position_and_Angle(int32_T *Position, double *angle, uint16_T status)
{
    //uint16_T errFlg = NOERROR;	
    int32_T last10_position;
#ifndef MATLAB_MEX_FILE
    
    /* calculate ElecAngle */
    *angle = (double)( (*Position - Angle_Offset) & (Motor_PolePairCount-1) ) / Motor_PolePairCount * 2*PI;
    
    if(status == 0){
        
         //*Position = *Position;
   
    }else{
        //error
        *Position = _last_position_storage[(_position_index + STORE_SIZE - 1)%STORE_SIZE];
    }
    
    last10_position = _last_position_storage[_position_index];
    _last_position_storage[_position_index] = *Position;
    _position_index = (_position_index + 1)%STORE_SIZE;
    
#endif
    
    return last10_position;
}


/*
 * Function name	: Tamagawa_InitPosition
 * Description		: 
 * Input			:
 * Output			:
 */
// uint16_T Tamagawa_InitPosition(uint32_T* PositionArray, uint16_T* pPositionArray)
// {
// #ifndef MATLAB_MEX_FILE
//     uint16_T i;
//     uint16_T Data[6];
//     uint16_T Status;
//     uint32_T Position;
//     
//     
//     Status = Tamagawa_GetPosition(Data);
//     
//     if (Status == NOERROR)
//     {
//         Position = (((uint32_T)Data[4] << 16) | ((uint32_T)Data[3] << 8) | (uint32_T)Data[2]) & 0x0001FFFF;
//     }
//     else
//     {
//         Position = 0x00000000;
//     }
//     
//     for (i=0; i<10; i++)
//         PositionArray[i] = Position;
// 
//     *pPositionArray = 0;
//     
//     return Status;
// #else
//     return NOERROR;
// #endif
// }


/* Private Functions ----------------------------------------------------*/
/*
 * Function name	: scib_Init
 * Description		: 
 * Input			:
 * Output			:
 */
static void scib_Init(void)
{
#ifndef MATLAB_MEX_FILE
	EALLOW;

	ScibRegs.SCICCR.bit.STOPBITS = 0;       /*Number of stop bits. (0: One stop bit, 1: Two stop bits)*/
	ScibRegs.SCICCR.bit.PARITY = 0;         /*Parity mode (0: Odd parity, 1: Even parity)*/
	ScibRegs.SCICCR.bit.PARITYENA = 0;      /*Enable Pary Mode */
	ScibRegs.SCICCR.bit.LOOPBKENA = 0;      /*Loop Back enable*/
	ScibRegs.SCICCR.bit.ADDRIDLE_MODE = 0;  /*ADDR/IDLE Mode control*/
	ScibRegs.SCICCR.bit.SCICHAR = 7;        /*Character length*/
	ScibRegs.SCICTL1.bit.RXERRINTENA = 0;   /*Disable receive error interrupt*/
	ScibRegs.SCICTL1.bit.SWRESET = 1;       /*Software reset*/
	ScibRegs.SCICTL1.bit.TXENA = 1;         /* SCI transmitter enable*/
	ScibRegs.SCICTL1.bit.RXENA = 1;         /* SCI receiver enable*/
	ScibRegs.SCIHBAUD = 0U;
	ScibRegs.SCILBAUD = 3U;
	
	ScibRegs.SCIPRI.bit.FREE = 1;           /* Free emulation mode control*/
	ScibRegs.SCIPRI.bit.SOFT = 0;           /* Interrupt priority select*/
	ScibRegs.SCIFFCT.bit.ABDCLR = 0;
	ScibRegs.SCIFFCT.bit.CDC = 0;
	ScibRegs.SCIFFTX.bit.SCIRST = 1;        /* SCI reset rx/tx channels*/
	ScibRegs.SCIFFTX.bit.SCIFFENA = 1;      /* SCI FIFO enhancements are enabled.*/
	ScibRegs.SCIFFTX.bit.TXFIFOXRESET = 1;  /* Re-enable transmit FIFO operation.*/
	ScibRegs.SCIFFRX.bit.RXFIFORESET = 1;   /* Re-enable receive FIFO operation.*/
    
    EDIS;
#endif
}


/*
 * Function name	: scib_Transmit
 * Description		: 
 * Input			:
 * Output			:
 */
static void scib_Transmit(char* pmsg, int msglen)
{
#ifndef MATLAB_MEX_FILE    
	uint16_T i;
  
	for (i=0; i<msglen; i++)
	{
        /* Wait if the buffer is full;*/
        while (ScibRegs.SCIFFTX.bit.TXFFST == 4);                            

        ScibRegs.SCITXBUF = pmsg[i];
    }

    /* Wait until all data in Tx FIFO are sent */ 
    while(ScibRegs.SCIFFTX.bit.TXFFST != 0);
    
    /* Wait until last byte is sent out */
    while(ScibRegs.SCICTL2.bit.TXEMPTY != 1);
#endif
}


/*
 * Function name	: scib_Receive
 * Description		: 
 * Input			:
 * Output			:
 */
static uint16_T scib_Receive(uint16_T *rcvBuff, uint16_T buffLen)
{
#ifndef MATLAB_MEX_FILE    
    uint16_T i, timeout;
    uint16_T errorVal = NOERROR;
  
    for (i=0; i<buffLen; i++)
        rcvBuff[i] = 0x0000;
  
    for (i=0; i<buffLen; i++)
    {
        timeout = 0;
        while (ScibRegs.SCIFFRX.bit.RXFFST == 0)
        {                                  /* wait until data received */
            if (timeout++ > 15)
                return TIMEOUT;
        }

        rcvBuff[i] = (ScibRegs.SCIRXBUF.all & 0x00FF);

        if (ScibRegs.SCIFFRX.bit.RXFFOVF == 1)/* detect FIFO overflow*/
        {
            ScibRegs.SCIFFRX.bit.RXFFOVRCLR = 1;

            ScibRegs.SCIFFRX.bit.RXFIFORESET = 0;    /* Reset the FIFO pointer to zero.   */
            ScibRegs.SCIFFRX.bit.RXFIFORESET = 1;    /* Re-enable receive FIFO operation. */
        }

        if (ScibRegs.SCIRXST.bit.FE)
            errorVal = FRAMERR;
        if (ScibRegs.SCIRXST.bit.PE)
            errorVal = PRTYERR;
        if (ScibRegs.SCIRXST.bit.OE)
            errorVal = OVRNERR;
        if (ScibRegs.SCIRXST.bit.BRKDT)
            errorVal = BRKDTERR;
    
        if (ScibRegs.SCIRXST.bit.RXERROR == 1)
        {
            ScibRegs.SCICTL1.bit.SWRESET = 1;
            ScibRegs.SCICTL1.bit.SWRESET = 0;
            ScibRegs.SCICTL1.bit.SWRESET = 1;
        }
    }

    return errorVal;
#else
    return NOERROR;
#endif
}
